#include <iostream>
#include <cmath>
#include <list>
#include <boost/numeric/mtl/mtl.hpp>
#include <cppunit/extensions/HelperMacros.h>
#include "../src/fmm.h"
#include "../src/geodesics.h"
#include "Test_geodesics.h"

#define PLANE5 "../../../data/plane5.obj" 


CPPUNIT_TEST_SUITE_REGISTRATION(Test_geodesics);

void Test_geodesics::setUp()
{
}
void Test_geodesics::tearDown()
{
}

void Test_geodesics::test_LU_solver()
{
//	CPPUNIT_FAIL("FAIL");
  	using namespace mtl;
	double data[][4]= {{2,3,4,5},
					{4,10,13,16},
					{6,25,38,46},
					{8,32,77,100}};
	
	unsigned size= 4;

	MATRIX  A(data);
	VECTOR v(size);
	for (unsigned i = 0; i < size; ++i)
	{
		v[i] = SCALAR (i);
	}
	VECTOR w(A * v);

	VECTOR solution(size);

	CPPUNIT_ASSERT (true == LU_solver(A, w, solution));
	for (unsigned i = 0; i < size; ++i)
	{
		CPPUNIT_ASSERT(fabs(v[i] - solution[i])< ERROR);
	}
}
void Test_geodesics::test_draw_geodesics_generic(const char * argv, int start_vertex_index, unsigned center_face_idx)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;
	
	//CPPUNIT_FAIL("FAIL");
	CPPUNIT_ASSERT(true == OpenMesh::IO::read_mesh(mesh, argv, ropt));

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	unsigned start_index = start_vertex_index; //sphere
	std::vector<Mesh::VertexHandle> initial;
	initial.push_back(mesh.vertex_handle(start_index));
	Mesh::Point end = mesh.point(mesh.vertex_handle(start_index));
	mesh.data(mesh.vertex_handle(start_index)).set_distance(0);
	fast_marching (mesh, initial);

	std::vector<Mesh::FaceHandle> end_faces;
	for (Mesh::VertexFaceIter vf_it = mesh.vf_iter(mesh.vertex_handle(start_index)); vf_it; ++vf_it)
	{
		end_faces.push_back(vf_it.handle());
		//std::cout<<vf_it.handle().idx()<<std::endl;
	}
	

	Mesh::FaceHandle start_face = mesh.face_handle(center_face_idx);
	Mesh::Point start(0,0,0);
	for (Mesh::FaceVertexIter fv_it  = mesh.fv_iter(start_face); fv_it; ++fv_it)
	{
		
		start [0] +=mesh.point(fv_it)[0];
		start [1] +=mesh.point(fv_it)[1];
		start [2] +=mesh.point(fv_it)[2];
	}
	start [0] /= 3.0f;
	start [1] /= 3.0f;
	start [2] /= 3.0f;
	

	std::vector<Mesh::Point> path;
	//draw_geodesics(mesh,  start, start_face, end, end_faces, 0.001, path);
	//generate_merged_mesh_geodesics(mesh, path);
}
void Test_geodesics::test_draw_geodesics_01()
{
	this->test_draw_geodesics_generic(PLANE5, 404, 500);
}
